#include <Arduino.h>
#include <Wire.h>
#include <WiFi.h>
#include "time.h"

// 配置参数
#define DEBUG_MODE 1
#define PCA9685_ADDR1 0x40
#define PCA9685_ADDR2 0x41
#define SERVO_MIN_PULSE 500
#define SERVO_MAX_PULSE 2500
const char* ssid = "Xiaomi";
const char* password = "mhf123321";
const char* ntpServer = "pool.ntp.org";

// 时区设置（北京时间UTC+8）
const long gmtOffset_sec = 8 * 3600;
const int daylightOffset_sec = 0;

// 函数声明
void initWiFi();
void printLocalTime();
void setupPCA9685(uint8_t addr);
void setPWMFreq(uint8_t addr, float freq);
void setServoAngle(uint8_t addr, uint8_t channel, uint8_t angle);
void updateAllServos(uint8_t angle);
void printServoLog(uint8_t ch, uint8_t angle, uint16_t pulse, uint16_t ticks40, uint16_t ticks41);

void setup() {
  Serial.begin(115200);
  Serial.setTimeout(200);
  while(!Serial); // 等待串口连接
  delay(1000);    // 额外稳定时间
  
  #if DEBUG_MODE
  Serial.println(F("\nSystem Boot..."));
  #endif

  // 初始化WiFi和时间
  initWiFi();
  configTime(gmtOffset_sec, daylightOffset_sec, ntpServer);

  // 初始化I2C
  Wire.begin(21, 22); // SDA=21, SCL=22
  Wire.setClock(100000);
  
  // 初始化PCA9685
  setupPCA9685(PCA9685_ADDR1);
  setupPCA9685(PCA9685_ADDR2);

  #if DEBUG_MODE
  printLocalTime();
  Serial.println(F("System Ready. 32 Channels Enabled"));
  #endif
}

void loop() {
  static uint8_t currentAngle = 0;
  static unsigned long lastTimePrint = 0;

  // 每5秒打印状态
  if (millis() - lastTimePrint >= 5000) {
    lastTimePrint = millis();
    printLocalTime();
    Serial.println(F("System Running..."));
  }

  // 更新所有舵机
  updateAllServos(currentAngle);

  // 角度递增逻辑
  currentAngle = (currentAngle >= 180) ? 0 : currentAngle + 30;
  delay(2000);
}

// WiFi初始化（纯英文输出）
void initWiFi() {
  #if DEBUG_MODE
  Serial.print(F("Connecting to WiFi: "));
  Serial.println(ssid);
  #endif
  
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    #if DEBUG_MODE
    Serial.print(F("."));
    #endif
  }
  #if DEBUG_MODE
  Serial.println(F("\nWiFi Connected"));
  Serial.print(F("IP Address: "));
  Serial.println(WiFi.localIP());
  #endif
}

// 时间打印（避免中文）
void printLocalTime() {
  struct tm timeinfo;
  if(!getLocalTime(&timeinfo)){
    Serial.println("[Time Not Synced]");
    return;
  }
  Serial.printf("[%02d:%02d:%02d]", 
               timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec);
}

// 修改后的舵机日志函数（带换行和时间开头）
void printServoLog(uint8_t ch, uint8_t angle, uint16_t pulse, uint16_t ticks40, uint16_t ticks41) {
  printLocalTime();  // 这已经会输出时间
  Serial.printf("\n0x40 CH%d -> Angle:%d° Pulse:%dμs Ticks:%d\n", ch, angle, pulse, ticks40);
  printLocalTime();  // 再次输出时间保持对齐
  Serial.printf("0x41 CH%d -> Angle:%d° Pulse:%dμs Ticks:%d\n\n", ch, angle, pulse, ticks41);
}

void updateAllServos(uint8_t angle) {
  // 计算脉冲和ticks
  angle = constrain(angle, 0, 180);
  uint16_t pulse = map(angle, 0, 180, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
  uint16_t ticks = (uint16_t)(pulse * 4096 / 20000.0);
  
  // 控制所有通道
  for(uint8_t ch = 0; ch < 16; ch++) {
    setServoAngle(PCA9685_ADDR1, ch, angle);
    setServoAngle(PCA9685_ADDR2, ch, angle);
  }
  
  #if DEBUG_MODE
  // 添加额外换行使输出更清晰
  Serial.println();
  printServoLog(0, angle, pulse, ticks, ticks);
  #endif
}

// PCA9685初始化
void setupPCA9685(uint8_t addr) {
  // MODE1设置
  Wire.beginTransmission(addr);
  Wire.write(0x00); // MODE1
  Wire.write(0x20); // AI=1
  if(Wire.endTransmission() != 0){
    #if DEBUG_MODE
    printLocalTime();
    Serial.printf("PCA9685 @ 0x%02X Init Failed!\n", addr);
    #endif
    return;
  }

  // 设置50Hz PWM
  setPWMFreq(addr, 50);
  delay(50);

  #if DEBUG_MODE
  printLocalTime();
  Serial.printf("PCA9685 @ 0x%02X Ready\n", addr);
  #endif
}

// PWM频率设置
void setPWMFreq(uint8_t addr, float freq) {
  freq = constrain(freq, 24, 1526);
  uint8_t prescale = round(25000000.0 / (4096 * freq)) - 1;

  Wire.beginTransmission(addr);
  Wire.write(0x00); // MODE1
  Wire.write(0x10); // SLEEP=1
  Wire.endTransmission();

  Wire.beginTransmission(addr);
  Wire.write(0xFE); // PRE_SCALE
  Wire.write(prescale);
  Wire.endTransmission();

  Wire.beginTransmission(addr);
  Wire.write(0x00); // MODE1
  Wire.write(0x20); // SLEEP=0, AI=1
  Wire.endTransmission();
}

// 舵机角度控制
void setServoAngle(uint8_t addr, uint8_t channel, uint8_t angle) {
  angle = constrain(angle, 0, 180);
  uint16_t pulse = map(angle, 0, 180, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
  uint16_t ticks = (uint16_t)(pulse * 4096 / 20000.0);

  #if DEBUG_MODE
  if(channel == 0) { // 仅记录通道0详情
    printServoLog(channel, angle, pulse, ticks, ticks);
  }
  #endif

  // 发送PWM指令
  Wire.beginTransmission(addr);
  Wire.write(0x06 + 4 * channel); // LEDn_ON_L
  Wire.write(0x00); // ON_L
  Wire.write(0x00); // ON_H
  Wire.write(ticks & 0xFF); // OFF_L
  Wire.write((ticks >> 8) & 0x0F); // OFF_H
  if(Wire.endTransmission() != 0){
    #if DEBUG_MODE
    printLocalTime();
    Serial.printf("0x%02X CH%d Command Error\n", addr, channel);
    #endif
  }
}